#!/usr/bin/env python2
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import PoseStamped
from tf.transformations import quaternion_from_euler

# 预定义目标点，可以是字典，也可以是列表
GOAL_POINTS = {
    1: (3.90, -7.35, -1.86),
    2: (2.60, -2.77, 3.14),
    3: (0.0,   0.0,   0.0),
}

def send_goal(goal_pub, goal_point):
    """
    发布导航目标点
    """
    goal = PoseStamped()
    goal.header.frame_id = "map"
    goal.header.stamp = rospy.Time.now()

    x, y, theta = goal_point

    goal.pose.position.x = x
    goal.pose.position.y = y

    # 将欧拉角theta转换为四元数
    quat = quaternion_from_euler(0, 0, theta)
    goal.pose.orientation.x = quat[0]
    goal.pose.orientation.y = quat[1]
    goal.pose.orientation.z = quat[2]
    goal.pose.orientation.w = quat[3]

    # 使用字符串格式化替代 f-string
    rospy.loginfo("Sending goal to x=%.2f, y=%.2f, theta=%.2f" % (x, y, theta))
    goal_pub.publish(goal)

def main():
    rospy.init_node("interactive_point_selection_py2")
    goal_pub = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=1)

    rospy.loginfo("Node started. Type the point index (e.g. '1') to move the robot, 'q' to quit.")

    # 在循环中等待用户输入 (Python2 使用 raw_input)
    while not rospy.is_shutdown():
        user_input = raw_input("Enter a point number (or 'q' to quit): ").strip()
        if user_input.lower() == 'q':
            rospy.loginfo("Quit command received, exiting...")
            break
        if not user_input.isdigit():
            rospy.logwarn("Invalid input. Please enter a number or 'q'.")
            continue

        point_index = int(user_input)
        if point_index not in GOAL_POINTS:
            rospy.logwarn("Point %d not in GOAL_POINTS." % point_index)
            continue

        # 发布目标点
        send_goal(goal_pub, GOAL_POINTS[point_index])

        # 此处可加上一些等待逻辑，或监听导航结果
        # 比如: rospy.sleep(1.0)

if __name__ == "__main__":
    try:
        main()
    except rospy.ROSInterruptException:
        pass
